#include "robot.h"
#include "Utility.h"
#include "geometry.h"
#include "environment.h"
#include "command.h"
#include "defs.h"
#include <tinyxml/tinyxml.h>
#include <urdf/model.h>
#include <urdf/link.h>
#include <math.h>
#include <assert.h>
#include <iostream>
#include <fstream>

#ifndef _WIN32
#define _strdup strdup
#endif

std::ofstream urdf_robot_debugger("debug.txt", std::ios::out);


short urdf_joint_id;
short urdf_param_id;
int box_idx, sphere_idx, cylinder_idx;

int compute_urdf_joint_num(urdf::Link* link)
{
	if(!link) return 0;
	
	urdf::Joint* jnt = link->parent_joint;
	
	int joint_num = 0;
	
	for(unsigned int i = 0; i < link->child_links.size(); ++i)
	{
		joint_num += compute_urdf_joint_num(link->child_links[i]);
	}
	
	if(jnt)
	{
		switch(jnt->type)
		{
		case urdf::Joint::CONTINUOUS:
		case urdf::Joint::REVOLUTE:
		case urdf::Joint::PRISMATIC:
			joint_num += 1;
			break;
		case urdf::Joint::FIXED:
			joint_num += 1;
			break;
		case urdf::Joint::FLOATING:
		case urdf::Joint::PLANAR:
			std::cout << "floating joint: can not handle well" << std::endl;
			break;
		}
	}
	
	return joint_num;
}

int compute_urdf_dof_num(urdf::Link* link)
{
	if(!link) return 0;
	
	urdf::Joint* jnt = link->parent_joint;
	
	int dim = 0;
	
	for(unsigned int i = 0; i < link->child_links.size(); ++i)
	{
		dim += compute_urdf_dof_num(link->child_links[i]);
	}
	
	if(jnt)
	{
		switch(jnt->type)
		{
		case urdf::Joint::CONTINUOUS:
		case urdf::Joint::REVOLUTE:
		case urdf::Joint::PRISMATIC:
			dim += 1;
			break;
		case urdf::Joint::FIXED:
			break;
		case urdf::Joint::PLANAR:
		case urdf::Joint::FLOATING:
			std::cout << "floating joint: can not handle well" << std::endl;
			break;
		}
	}
	
	return dim;
}

Transform urdf2transform(urdf::Pose const& pose)
{
	Transform m;
	m.m_T[0] = (float)pose.position.x;
	m.m_T[1] = (float)pose.position.y;
	m.m_T[2] = (float)pose.position.z;
	double r, p, y;
	pose.rotation.getRPY(r, p, y);
	double ca1, cb1, cc1, sa1, sb1, sc1;
	ca1 = cos(y);
	sa1 = sin(y);
	cb1 = cos(p);
	sb1 = sin(p);
	cc1 = cos(r);
	sc1 = sin(r);
	m.m_R[0][0] = (float)(ca1 * cb1);
	m.m_R[0][1] = (float)(ca1 * sb1 * sc1 - sa1 * cc1);
	m.m_R[0][2] = (float)(ca1 * sb1 * cc1 + sa1 * sc1);
	m.m_R[1][0] = (float)(sa1 * cb1);
	m.m_R[1][1] = (float)(sa1 * sb1 * sc1 + ca1 * cc1);
	m.m_R[1][2] = (float)(sa1 * sb1 * cc1 - ca1 * sc1);
	m.m_R[2][0] = (float)(-sb1);
	m.m_R[2][1] = (float)(cb1 * sc1);
	m.m_R[2][2] = (float)(cb1 * cc1);
	
	return m;
}

void modelTransformUrdf(urdf::Pose const& pose, ModelInstance* model)
{
	Transform t = urdf2transform(pose);
	float a[3], b[3];
	for(unsigned int i = 0; i < model->nVerts; ++i)
	{
		a[0] = model->verlist[i].x();
		a[1] = model->verlist[i].y();
		a[2] = model->verlist[i].z();
		
		t.apply(b, a);
		model->verlist[i] = SSEVertex(b[0], b[1], b[2]);
	}
}

void traverse_urdf_tree(urdf::Link* link, ArticulatedRobot::Joint* joints, EnvironmentBase* penv)
{
	if(!link) return;
	
	urdf::Joint* jnt = link->parent_joint;
	
	if(jnt)
	{
		urdf::Pose o = jnt->parent_to_joint_origin_transform;
		
		switch(jnt->type)
		{
		case urdf::Joint::FIXED:
		{
			joints[urdf_joint_id].name = jnt->name;
			joints[urdf_joint_id].j_min = 0;
			joints[urdf_joint_id].j_max = 0;
			joints[urdf_joint_id].joint_type = JT_FIXED;
			joints[urdf_joint_id].axis[0] = 0;
			joints[urdf_joint_id].axis[1] = 0;
			joints[urdf_joint_id].axis[2] = 0;
			joints[urdf_joint_id].T0 = urdf2transform(jnt->parent_to_joint_origin_transform);
			joints[urdf_joint_id].Tq.set_identity();
			joints[urdf_joint_id].param_idx = ROBOT_NOPARAM_NODE;
			joints[urdf_joint_id].model = NULL;
		}
		break;
		case urdf::Joint::REVOLUTE:
		{
			joints[urdf_joint_id].name = jnt->name;
			joints[urdf_joint_id].j_min = (float)jnt->limits->lower;
			joints[urdf_joint_id].j_max = (float)jnt->limits->upper;
			joints[urdf_joint_id].joint_type = JT_REVOLUTE;
			joints[urdf_joint_id].axis[0] = (float)jnt->axis.x;
			joints[urdf_joint_id].axis[1] = (float)jnt->axis.y;
			joints[urdf_joint_id].axis[2] = (float)jnt->axis.z;
			joints[urdf_joint_id].T0 = urdf2transform(jnt->parent_to_joint_origin_transform);
			joints[urdf_joint_id].Tq.set_identity();
			joints[urdf_joint_id].param_idx = urdf_param_id;
			urdf_param_id++;
			joints[urdf_joint_id].model = NULL;
		}
		break;
		case urdf::Joint::CONTINUOUS:
		{
			joints[urdf_joint_id].name = jnt->name;
			joints[urdf_joint_id].j_min = 0;
			joints[urdf_joint_id].j_max = MATH_PI * 2.0f;
			joints[urdf_joint_id].joint_type = JT_CONTINUOUS;
			joints[urdf_joint_id].axis[0] = (float)jnt->axis.x;
			joints[urdf_joint_id].axis[1] = (float)jnt->axis.y;
			joints[urdf_joint_id].axis[2] = (float)jnt->axis.z;
			joints[urdf_joint_id].T0 = urdf2transform(jnt->parent_to_joint_origin_transform);
			joints[urdf_joint_id].Tq.set_identity();
			joints[urdf_joint_id].param_idx = urdf_param_id;
			urdf_param_id++;
			joints[urdf_joint_id].model = NULL;
		}
		break;
		case urdf::Joint::PRISMATIC:
		{
			joints[urdf_joint_id].name = jnt->name;
			joints[urdf_joint_id].j_min = jnt->limits->lower;
			joints[urdf_joint_id].j_max = jnt->limits->upper;
			joints[urdf_joint_id].joint_type = JT_PRISMATIC;
			joints[urdf_joint_id].axis[0] = (float)jnt->axis.x;
			joints[urdf_joint_id].axis[1] = (float)jnt->axis.y;
			joints[urdf_joint_id].axis[2] = (float)jnt->axis.z;
			joints[urdf_joint_id].T0 = urdf2transform(jnt->parent_to_joint_origin_transform);
			joints[urdf_joint_id].Tq.set_identity();
			joints[urdf_joint_id].param_idx = urdf_param_id;
			urdf_param_id++;
			joints[urdf_joint_id].model = NULL;
		}
		break;
		default:
		{
			std::cout << "ERROR: joint type not supported yet!" << std::endl;
		}
		}
		
		if(link->visual)
		{
			if(link->visual->geometry)
			{
				switch(link->visual->geometry->type)
				{
				case urdf::Geometry::BOX:
				{
					ModelInstance* model = new ModelInstance;
					urdf::Box* param = ((urdf::Box*)(link->visual->geometry));
					urdf::Pose pose = link->visual->origin;
					model->makeBox((float)param->dim.x, (float)param->dim.y, (float)param->dim.z);
					modelTransformUrdf(pose, model);
					model->modelType = ROBOT_MODEL;
					char buffer[33];
					//itoa(box_idx, buffer, 10);
					snprintf(buffer, 33, "%d", box_idx);
                    box_idx++;
					model->modelFileName = std::string("box") + std::string(buffer);
					joints[urdf_joint_id].model = model;
					penv->add_model(model);
				}
				break;
				case urdf::Geometry::CYLINDER:
				{
					ModelInstance* model = new ModelInstance;
					urdf::Cylinder* param = ((urdf::Cylinder*)(link->visual->geometry));
					urdf::Pose pose = link->visual->origin;
					model->makeCylinder(param->radius, param->length);
					modelTransformUrdf(pose, model);
					model->modelType = ROBOT_MODEL;
					char buffer[33];
					//itoa(cylinder_idx, buffer, 10);
					snprintf(buffer, 33, "%d", cylinder_idx);
                    cylinder_idx++;
					model->modelFileName = std::string("cylinder") + std::string(buffer);
					joints[urdf_joint_id].model = model;
					penv->add_model(model);
				}
				break;
				case urdf::Geometry::SPHERE:
				{
					ModelInstance* model = new ModelInstance;
					urdf::Sphere* param = ((urdf::Sphere*)(link->visual->geometry));
					urdf::Pose pose = link->visual->origin;
					model->makeSphere(param->radius);
					modelTransformUrdf(pose, model);
					model->modelType = ROBOT_MODEL;
					char buffer[33];
					//itoa(sphere_idx, buffer, 10);
					snprintf(buffer, 33, "%d", sphere_idx);
                    sphere_idx++;
					model->modelFileName = std::string("sphere") + std::string(buffer);
					joints[urdf_joint_id].model = model;
					penv->add_model(model);
				}
				break;
				case urdf::Geometry::MESH:
				{
					std::string full_path_file_name = ((urdf::Mesh*)(link->visual->geometry))->filename;
					ModelInstance* model = new ModelInstance;
					urdf::Pose pose = link->visual->origin;
					model->loadSTLB(full_path_file_name.c_str(), 1.0f);
					modelTransformUrdf(pose, model);
					model->modelFileName = (((urdf::Mesh*)(link->visual->geometry))->filename).c_str();
					model->modelType = ROBOT_MODEL;
					joints[urdf_joint_id].model = model;
					penv->add_model(model);
				}
				break;
				default:
					std::cout << "geometry type not supported!" << std::endl;
				}
				
			}
		}
		
		//std::cout << urdf_joint_id << std::endl;
		
		switch(jnt->type)
		{
		case urdf::Joint::FIXED:
		case urdf::Joint::REVOLUTE:
		case urdf::Joint::PRISMATIC:
		case urdf::Joint::CONTINUOUS:
		{
			joints[urdf_joint_id].parent_idx = ROBOT_NOPARENT_NODE;
			if(link->getParent())
			{
				for(int t = 0; t < urdf_joint_id; ++t)
				{
					if(joints[t].name == link->getParent()->parent_joint->name)
					{
						joints[urdf_joint_id].parent_idx = (short)t;
						break;
					}
				}
			}
			urdf_joint_id++;
		}
		break;
		default:
			;
		}
	}
	
	
	for(int i = 0; i < link->child_links.size(); ++i)
	{
		if(!link->child_links[i]) continue;
		traverse_urdf_tree(link->child_links[i], joints, penv);
	}
}

void print_urdf_tree(urdf::Link* link)
{
	urdf_robot_debugger << "link name: " << link->name << std::endl;
	if(link->visual)
	{
		if(link->visual->geometry)
		{
			switch(link->visual->geometry->type)
			{
			case urdf::Geometry::SPHERE:
				urdf_robot_debugger << "geometry: sphere" << std::endl;
				break;
			case urdf::Geometry::BOX:
				urdf_robot_debugger << "geometry: box" << std::endl;
				break;
			case urdf::Geometry::CYLINDER:
				urdf_robot_debugger << "geometry: cylinder" << std::endl;
				break;
			case urdf::Geometry::MESH:
				urdf_robot_debugger << "geometry: mesh, " << ((urdf::Mesh*)(link->visual->geometry))->filename << std::endl;
				break;
			}
		}
		else
		{
			urdf_robot_debugger << "link " << link->name << " does not have a geometry" << std::endl;
		}
		
		urdf::Pose o = link->visual->origin;
		urdf_robot_debugger << o.position.x << " " << o.position.y << " " << o.position.z << std::endl;
		double r, p, y;
		o.rotation.getRPY(r, p, y);
		urdf_robot_debugger << r << " " << p << " " << y << std::endl;
	}
	else
	{
		urdf_robot_debugger << "link " << link->name << " does not have a visual" << std::endl;
	}
	
	urdf::Joint* jnt = link->parent_joint;
	if(!jnt)
	{
		urdf_robot_debugger << "null joint" << std::endl;
	}
	else
	{
		urdf_robot_debugger << "joint " << jnt->name;
		switch(jnt->type)
		{
		case urdf::Joint::CONTINUOUS:
			urdf_robot_debugger << " continuous " << std::endl;
			break;
		case urdf::Joint::FLOATING:
			urdf_robot_debugger << " floating " << std::endl;
			break;
		case urdf::Joint::FIXED:
			urdf_robot_debugger << " fixed " << std::endl;
			break;
		case urdf::Joint::PLANAR:
			urdf_robot_debugger << " planar " << std::endl;
			break;
		case urdf::Joint::PRISMATIC:
			urdf_robot_debugger << " prismatic " << std::endl;
			break;
		case urdf::Joint::REVOLUTE:
			urdf_robot_debugger << " revolute " << std::endl;
			break;
		case urdf::Joint::UNKNOWN:
			urdf_robot_debugger << " unknown " << std::endl;
			break;
		}
		
		switch(jnt->type)
		{
		case urdf::Joint::CONTINUOUS:
		case urdf::Joint::PLANAR:
		case urdf::Joint::PRISMATIC:
		case urdf::Joint::REVOLUTE:
			urdf_robot_debugger << "axis: " << jnt->axis.x << " " << jnt->axis.y << " " << jnt->axis.z << std::endl;
			break;
		default:
			urdf_robot_debugger << "axis is not defined " << std::endl;
		}
		
		urdf::Pose o = jnt->parent_to_joint_origin_transform;
		urdf_robot_debugger << o.position.x << " " << o.position.y << " " << o.position.z << std::endl;
		double r, p, y;
		o.rotation.getRPY(r, p, y);
		urdf_robot_debugger << r << " " << p << " " << y << std::endl;
	}
	
	urdf_robot_debugger << std::endl;
	
	for(int i = 0; i < link->child_links.size(); ++i)
	{
		if(!link->child_links[i]) continue;
		print_urdf_tree(link->child_links[i]);
	}
	
}

RobotBase::RobotBase(EnvironmentBase* penv_)
{
	dim_state = 0;
	state_lower_bounds = NULL;
	state_upper_bounds = NULL;
	dist_state_coeffs = NULL;
	penv = penv_;
	robot_type = RT_BASE;
}

RobotBase::~RobotBase()
{
	delete [] state_lower_bounds;
	state_lower_bounds = NULL;
	delete [] state_upper_bounds;
	state_upper_bounds = NULL;
	delete [] dist_state_coeffs;
	dist_state_coeffs = NULL;
}

EnvironmentBase* RobotBase::get_env() const
{
	return penv;
}

void RobotBase::set_bbox_state(float lower[], float upper[])
{
	for(int i = 0; i < dim_state; ++i)
	{
		state_lower_bounds[i] = lower[i];
		state_upper_bounds[i] = upper[i];
	}
}

void RobotBase::set_dist_state_coeffs(float coeffs[])
{
	for(int i = 0; i < dim_state; ++i)
	{
		dist_state_coeffs[i] = coeffs[i];
	}
}

void RobotBase::set_dist_state_coeffs(float coeff)
{
	for(int i = 0; i < dim_state; ++i)
	{
		dist_state_coeffs[i] = coeff;
	}
}

RigidBodyRobot3T::RigidBodyRobot3T(EnvironmentBase* penv_) : RobotBase(penv_)
{
	dim_state = 3;
	state_lower_bounds = new float[dim_state];
	state_upper_bounds = new float[dim_state];
	dist_state_coeffs = new float[dim_state];
	robot_type = RT_RIGID_3T;
}

void RigidBodyRobot3T::load(std::string const& rob_file_name)
{
	//for articulate body directly load the model file
	model = new ModelInstance;
	std::string fullname = rob_file_name;
	model->load(fullname.c_str(), (penv->m_cmd)->m_robot_scale);
	model->modelFileName = rob_file_name;
	penv->add_model(model);
	set_bbox_state(penv->m_cmd->m_dof_lower_bounds, penv->m_cmd->m_dof_upper_bounds);
}

RigidBodyRobot3T3R::RigidBodyRobot3T3R(EnvironmentBase* penv_) : RobotBase(penv_)
{
	dim_state = 6;
	state_lower_bounds = new float[dim_state];
	state_upper_bounds = new float[dim_state];
	dist_state_coeffs = new float[dim_state];
	robot_type = RT_RIGID_3T3R;
}

void RigidBodyRobot3T3R::load(std::string const& rob_file_name)
{
	//for articulate body directly load the model file
	model = new ModelInstance;
	std::string fullname = rob_file_name;
	std::cout << "robot scale " << (penv->m_cmd)->m_robot_scale << std::endl;
	model->load(fullname.c_str(), (penv->m_cmd)->m_robot_scale);
	model->modelFileName = rob_file_name;
	penv->add_model(model);
	set_bbox_state(penv->m_cmd->m_dof_lower_bounds, penv->m_cmd->m_dof_upper_bounds);
}


ArticulatedRobot::RobFileScanner::RobFileScanner(const char* fname)
{
	this->fname = fname;
	f = new std::ifstream(fname);
	if(!(*f))
	{
		delete f;
		std::cout << "Could not find file " << fname << std::endl;
		return;
	}
	std::string s1, s2, s3;
	(*f) >> s1;
	(*f) >> s2;
	(*f) >> s3;
	if(s1 != "#MPK" || s2 != "v1.0" || s3 != "robot")
	{
		std::cout << "Error: not an MPK v1.0 robot file: " << fname << std::endl;
		return;
	}
}


bool ArticulatedRobot::RobFileScanner::get_token(std::string& s, const char* expected, bool exit_on_eof)
{
	if(!read_string(s, expected))
	{
		if(!exit_on_eof) return false;
		std::cout << "Problem reading file " << fname << std::endl;
		return false;
	}
	
	if(expected && s != expected)
	{
		std::cout << "Error in file " << fname
		          << ": expected '" << expected << "' but read '" << s << "'" << std::endl;
		return false;
	}
	return true;
}

void ArticulatedRobot::RobFileScanner::get_string_token(std::string& s)
{
	if(!read_string(s))
	{
		std::cout << "Problem reading file " << fname << std::endl;
		return;
	}
	if(s[0] != '"' || s[s.length()-1] != '"')
	{
		std::cout << "Error in file " << fname
		          << ": expected string delimiter \"" << std::endl;
		return;
	}
	s.replace(0, 1, "");
	s.replace(s.length() - 1, 1, "");
}

bool ArticulatedRobot::RobFileScanner::read_string(std::string& s, const char* expected)
{
	s = "";
	int c;
	while(1)
	{
		c = f->get();
		if(c == EOF) return false;
		if(c == '#')
		{
			while(c != EOF && c != 10)
				c = f->get();
		}
		if(!isspace(c)) break;
	}
	s += c;
	while(1)
	{
		if(expected && s == expected) break;
		c = f->get();
		if(c == EOF) break;
		if(isspace(c) || c == '{' || c == '}' || c == '#')
		{
			f->putback(c);
			break;
		}
		s += c;
	}
	return true;
}


void ArticulatedRobot::load_rob(std::string const& rob_file_name)
{
	std::string full_rob_file_name = rob_file_name;
	RobFileScanner pass1(full_rob_file_name.c_str());
	std::string s;
	std::vector<std::string> joint_names;
	while(pass1.get_token(s, 0, false))
	{
		if(s == "joint")
		{
			pass1.get_token(s);
			joint_names.push_back(s);
		}
		pass1.get_token(s, "{");
		int cnt = 1;
		while(cnt)
		{
			pass1.get_token(s);
			if(s == "{") cnt++;
			else if(s == "}") cnt--;
		}
	}
	RobFileScanner pass2(full_rob_file_name.c_str());
	
	unsigned int dim = joint_names.size();
	std::cout << dim << std::endl;
	
	if(state_lower_bounds) delete [] state_lower_bounds;
	state_lower_bounds = NULL;
	if(state_upper_bounds) delete [] state_upper_bounds;
	state_upper_bounds = NULL;
	if(dist_state_coeffs) delete [] dist_state_coeffs;
	dist_state_coeffs = NULL;
	if(joints) delete [] joints;
	joints = NULL;
	noSelfcollision_info.clear();
	
	joints = new Joint[dim];
	state_lower_bounds = new float[dim];
	state_upper_bounds = new float[dim];
	dist_state_coeffs = new float[dim];
	
	
	int i = -1;
	while(pass2.get_token(s, 0, false))
	{
		if(s == "joint")
		{
			i++;
			joints[i].parent_idx = ROBOT_NOPARENT_NODE;
			joints[i].T0.set_identity();
			joints[i].Tq.set_identity();
			joints[i].model = NULL;
			joints[i].joint_type = JT_FIXED; // default: fixed
			joints[i].param_idx = ROBOT_NOPARAM_NODE; // default: -1 (not corresponding to configuration)
			
			pass2.get_token(s, joint_names[i].c_str());
			joints[i].name = copy_string(s.c_str());
			pass2.get_token(s, "{");
			
			while(s != "}")
			{
				pass2.get_token(s);
				if(s == "parent")
				{
					pass2.get_token(s);
					int j = find_name(s, joint_names);
					if(j >= 0)
						joints[i].parent_idx = (short)j;
					else
					{
						std::cout << "Error in robot file " << rob_file_name << ": parent "
						          << s << " of " << joint_names[i] << " undefined." << std::endl;
						return;
					}
				}
				else if(s == "ConstTransl")
				{
					pass2.get_token(s);
					float tx = (float)atof(s.c_str());
					pass2.get_token(s);
					float ty = (float)atof(s.c_str());
					pass2.get_token(s);
					float tz = (float)atof(s.c_str());
					joints[i].T0.append(make_transl_transform(tx, ty, tz));
				}
				else if(s == "ConstRot")
				{
					pass2.get_token(s);
					float ax = (float)atof(s.c_str());
					pass2.get_token(s);
					float ay = (float)atof(s.c_str());
					pass2.get_token(s);
					float az = (float)atof(s.c_str());
					pass2.get_token(s);
					float ang = (float)atof(s.c_str());
					joints[i].T0.append(make_rotate_transform(ax, ay, az, ang));
				}
				else if(s == "Transl1")
				{
					pass2.get_token(s);
					float tx = (float)atof(s.c_str());
					pass2.get_token(s);
					float ty = (float)atof(s.c_str());
					pass2.get_token(s);
					float tz = (float)atof(s.c_str());
					pass2.get_token(s);
					float j_min = (float)atof(s.c_str());
					pass2.get_token(s);
					float j_max = (float)atof(s.c_str());
					
					float len = sqrt(tx * tx + ty * ty + tz * tz);
					assert(len > 0);
					tx /= len;
					ty /= len;
					tz /= len;
					
					joints[i].axis[0] = tx;
					joints[i].axis[1] = ty;
					joints[i].axis[2] = tz;
					joints[i].j_min = j_min;
					joints[i].j_max = j_max;
					state_lower_bounds[i] = j_min;
					state_upper_bounds[i] = j_max;
					joints[i].joint_type = JT_PRISMATIC; // translate: default prismatic
				}
				else if(s == "Rot1")
				{
					pass2.get_token(s);
					float ax = (float)atof(s.c_str());
					pass2.get_token(s);
					float ay = (float)atof(s.c_str());
					pass2.get_token(s);
					float az = (float)atof(s.c_str());
					pass2.get_token(s);
					float j_min = (float)atof(s.c_str());
					pass2.get_token(s);
					float j_max = (float)atof(s.c_str());
					
					float len = sqrt(ax * ax + ay * ay + az * az);
					assert(len > 0);
					ax /= len;
					ay /= len;
					az /= len;
					
					joints[i].axis[0] = ax;
					joints[i].axis[1] = ay;
					joints[i].axis[2] = az;
					joints[i].j_min = j_min;
					joints[i].j_max = j_max;
					state_lower_bounds[i] = j_min;
					state_upper_bounds[i] = j_max;
					joints[i].joint_type = JT_CONTINUOUS; // rotate: default continuous
				}
				else if(s == "ConstTransl_Rot1")
				{
					pass2.get_token(s);
					float tx = (float)atof(s.c_str());
					pass2.get_token(s);
					float ty = (float)atof(s.c_str());
					pass2.get_token(s);
					float tz = (float)atof(s.c_str());
					pass2.get_token(s);
					float ax = (float)atof(s.c_str());
					pass2.get_token(s);
					float ay = (float)atof(s.c_str());
					pass2.get_token(s);
					float az = (float)atof(s.c_str());
					pass2.get_token(s);
					float j_min = (float)atof(s.c_str());
					pass2.get_token(s);
					float j_max = (float)atof(s.c_str());
					
					joints[i].T0.append(make_transl_transform(tx, ty, tz));
					
					float len = sqrt(ax * ax + ay * ay + az * az);
					assert(len > 0);
					ax /= len;
					ay /= len;
					az /= len;
					
					joints[i].axis[0] = ax;
					joints[i].axis[1] = ay;
					joints[i].axis[2] = az;
					joints[i].j_min = j_min;
					joints[i].j_max = j_max;
					state_lower_bounds[i] = j_min;
					state_upper_bounds[i] = j_max;
					joints[i].joint_type = JT_CONTINUOUS; // rotate: default continuous
				}
				else if(s == "param")
				{
					pass2.get_token(s);
					joints[i].param_idx = (short)atoi(s.c_str());
				}
				else if(s == "type")
				{
					pass2.get_token(s);
					if(s == "prismatic")
					{
						joints[i].joint_type = JT_PRISMATIC;
					}
					else if(s == "rotate")
					{
						joints[i].joint_type = JT_REVOLUTE;
					}
					else if(s == "continuous")
					{
						joints[i].joint_type = JT_CONTINUOUS;
					}
					else if(s == "fixed")
					{
						joints[i].joint_type = JT_FIXED;
					}
					else
					{
						std::cout << "ERROR: bad joint type" << std::endl;
					}
				}
				else if(s == "model0")
				{
					pass2.get_string_token(s);
					joints[i].model = new ModelInstance;
					joints[i].model->modelFileName = s;
					std::string fullname = std::string(s);
					joints[i].model->load(fullname.c_str(), 1.0);
					joints[i].model->modelType = ROBOT_MODEL;
					penv->add_model(joints[i].model);
					std::cout << joints[i].model->nVerts << std::endl;
					std::cout << joints[i].model->nTris << std::endl;
				}
				else if(s != "}")
				{
					std::cout << "Error: unexpected token '" << s << "' in file " << rob_file_name << std::endl;
					return;
				}
			}
		}
		else if(s == "noselfcoll")
		{
			pass2.get_token(s, "{");
			while(1)
			{
				pass2.get_token(s);
				if(s == "}") break;
				std::string s2;
				pass2.get_token(s2);
				int j1 = find_name(s, joint_names);
				int j2 = find_name(s2, joint_names);
				if(j1 < 0 || j2 < 0)
				{
					std::cout << "Error in robot file " << rob_file_name
					          << ": undefined joint in self-collision set." << std::endl;
					return;
				}
				noSelfcollision_info.push_back(NoSelfCollisionPair(j1, j2));
			}
			
		}
	}
	
	dim_state = dim;
	joint_num = dim;
	
	penv->m_cmd->m_dof_lower_bounds = new float[dim_state];
	penv->m_cmd->m_dof_upper_bounds = new float[dim_state];
	
	for(int i = 0; i < dim_state; ++i)
	{
		penv->m_cmd->m_dof_lower_bounds[i] = state_lower_bounds[i];
		penv->m_cmd->m_dof_upper_bounds[i] = state_upper_bounds[i];
	}
	
	if(!penv->m_cmd->m_dof_topologies)
	{
		penv->m_cmd->m_dof_topologies = new DOFTopoType[dim_state];
		for(int i = 0; i < dim_state; i++)
		{
			if(joints[i].joint_type == JT_CONTINUOUS)
				penv->m_cmd->m_dof_topologies[i] = DTT_CIRCLE;
			else
				penv->m_cmd->m_dof_topologies[i] = DTT_EUCLIDEAN;
		}
	}
	
	// print structures
	//for(int i = 0; i < dim_state; ++i)
	//{
	//	std::cout << joint_names[i] << std::endl;
	//	std::cout << joints[i].axis[0] << " " << joints[i].axis[1] << " " << joints[i].axis[2] << std::endl;
	//	std::cout << joints[i].param_idx << std::endl;
	//	std::cout << joints[i].parent_idx << std::endl;
	//	if(joints[i].model)
	//	{
	//		std::cout << joints[i].model->nTris << " " << joints[i].model->nVerts << std::endl;
	//	}
	//	else
	//	{
	//		std::cout << "no model" << std::endl;
	//	}
	
	//	//std::cout << joints[i].T0 << std::endl;
	//	for(int j = 0; j < 3; ++j)
	//		std::cout << joints[i].T0.m_T[j] << " ";
	//	std::cout << std::endl;
	//	for(int j = 0; j < 3; ++j)
	//	{
	//		for(int k = 0; k < 3; ++k)
	//		{
	//			std::cout << joints[i].T0.m_R[j][k] << " ";
	//		}
	//		std::cout << std::endl;
	//	}
	//}
}


void ArticulatedRobot::load_urdf(std::string const& rob_file_name)
{
	std::string full_rob_file_name = rob_file_name;
	TiXmlDocument robot_model_xml;
	robot_model_xml.LoadFile(full_rob_file_name.c_str());
	TiXmlElement* robot_xml = robot_model_xml.FirstChildElement("robot");
	if(!robot_xml)
	{
		printf("ERROR: Could not load the xml into TiXmlElement\n");
		return;
	}
	
	urdf::Model robot;
	if(!robot.initXml(robot_xml))
	{
		printf("ERROR: Model parsing the xml failed\n");
		return;
	}
	
	printf("robot name is: %s\n", robot.getName().c_str());
	printf("---------- Successfully Parsed XML ---------------\n");
	
	urdf::Link* root_link = robot.getRoot();
	if(!root_link)
	{
		printf("ERROR: robot root is null\n");
		return;
	}
	
#ifdef GPLANNER_DEBUG
	print_urdf_tree(root_link);
#endif
	joint_num = compute_urdf_joint_num(root_link);
#ifdef GPLANNER_DEBUG
	std::cout << "joint num: " << joint_num << std::endl;
#endif
	joints = new Joint[joint_num];
	
	urdf_joint_id = 0;
	urdf_param_id = 0;
	sphere_idx = 0;
	box_idx = 0;
	cylinder_idx = 0;
	
	traverse_urdf_tree(root_link, joints, penv);
	
	
	// self collision
	//std::vector<urdf::Link*> links;
	//robot.getLinks(links);
	//for(int i = 0; i < robot.noSelfcollision_info.size(); ++i)
	//{
	//	int idx = -1;
	//	for(int j = 0; j < links.size(); ++j)
	//	{
	//		if(links[j]->name == robot.noSelfcollision_info[i].link_a)
	//		{
	//			idx = j;
	//			break;
	//		}
	//	}
	
	//	int idy = -1;
	//	for(int j = 0; j < links.size(); ++j)
	//	{
	//		if(links[j]->name == robot.noSelfcollision_info[i].link_b)
	//		{
	//			idy = j;
	//			break;
	//		}
	//	}
	
	//	noSelfcollision_info.push_back(NoSelfCollisionPair(idx, idy));
	//}
	printf("size %d\n", robot.noSelfcollision_info.size());
	for(int i = 0; i < robot.noSelfcollision_info.size(); ++i)
	{
		int idx = -1;
		for(int j = 0; j < joint_num; ++j)
		{
			if(joints[j].name == robot.noSelfcollision_info[i].link_a)
			{
				idx = j;
				break;
			}
		}
		
		int idy = -1;
		for(int j = 0; j < joint_num; ++j)
		{
			if(joints[j].name == robot.noSelfcollision_info[i].link_b)
			{
				idy = j;
				break;
			}
		}
		noSelfcollision_info.push_back(NoSelfCollisionPair(idx, idy));
	}
	
	
	//for(int i = 0; i < joint_num; ++i)
	//{
	//	std::cout << joints[i].axis[0] << " " << joints[i].axis[1] << " " << joints[i].axis[2] << std::endl;
	//	std::cout << joints[i].T0.m_T[0] << " " << joints[i].T0.m_T[1] << " " << joints[i].T0.m_T[2] << std::endl;
	//	std::cout << joints[i].T0.m_R[0][0] << " " << joints[i].T0.m_R[0][1] << " " << joints[i].T0.m_R[0][2] << std::endl;
	//	std::cout << joints[i].T0.m_R[1][0] << " " << joints[i].T0.m_R[1][1] << " " << joints[i].T0.m_R[1][2] << std::endl;
	//	std::cout << joints[i].T0.m_R[2][0] << " " << joints[i].T0.m_R[2][1] << " " << joints[i].T0.m_R[2][2] << std::endl;
	//}
	
	//for(int i = 0; i < joint_num; ++i)
	//{
	//	std::cout << i << " " << joints[i].param_idx << " " << joints[i].parent_idx << std::endl;
	//}
	
	dim_state = compute_urdf_dof_num(root_link);
#ifdef GPLANNER_DEBUG
	std::cout << "dof num: " << dim_state << std::endl;
#endif
	
	state_lower_bounds = new float[dim_state];
	state_upper_bounds = new float[dim_state];
	dist_state_coeffs = new float[dim_state];
	
	for(int i = 0; i < joint_num; ++i)
	{
		short param_idx = joints[i].param_idx;
		if(param_idx != ROBOT_NOPARAM_NODE)
		{
			state_lower_bounds[param_idx] = joints[i].j_min;
			state_upper_bounds[param_idx] = joints[i].j_max;
		}
	}
	
	penv->m_cmd->m_dof_lower_bounds = new float[dim_state];
	penv->m_cmd->m_dof_upper_bounds = new float[dim_state];
	
	for(int i = 0; i < dim_state; ++i)
	{
		penv->m_cmd->m_dof_lower_bounds[i] = state_lower_bounds[i];
		penv->m_cmd->m_dof_upper_bounds[i] = state_upper_bounds[i];
	}
	
	if(!penv->m_cmd->m_dof_topologies)
	{
		penv->m_cmd->m_dof_topologies = new DOFTopoType[dim_state];
		for(int i = 0; i < joint_num; i++)
		{
			short param_id = joints[i].param_idx;
			if(param_id != ROBOT_NOPARAM_NODE)
			{
				if(joints[i].joint_type == JT_CONTINUOUS)
					penv->m_cmd->m_dof_topologies[param_id] = DTT_CIRCLE;
				else
					penv->m_cmd->m_dof_topologies[param_id] = DTT_EUCLIDEAN;
			}
		}
	}
	
	//for(int i = 0; i < joint_num; ++i)
	//{
	//	std::cout << i << " " << joints[i].param_idx << std::endl;
	//}
	//
	//std::cout << "mesh info" << std::endl;
	//for(int i = 0; i < joint_num; ++i)
	//{
	//	if(joints[i].model)
	//	{
	//		std::cout << " " << joints[i].model->nVerts << " " << joints[i].model->nTris;
	//	}
	//	std::cout << std::endl;
	//}
}

void ArticulatedRobot::load(std::string const& rob_file_name)
{
	char* extension = _strdup(strrchr(rob_file_name.c_str(), '.'));
	strtolower(extension);
	
	if(strcmp(extension, ".rob") == 0)
	{
		load_rob(rob_file_name);
	}
	else if(strcmp(extension, ".urdf") == 0)
	{
		load_urdf(rob_file_name);
	}
}

void ArticulatedRobot::save_rob(std::string const& rob_file_name)
{
	std::string full_rob_file_name = rob_file_name;
	FILE* file = fopen(full_rob_file_name.c_str(), "w");
	fprintf(file, "#MPK v1.0 robot\n");
	
	for(int i = 0; i < joint_num; ++i)
	{
		Joint joint = joints[i];
		fprintf(file, "joint %s\n", joint.name.c_str());
		fprintf(file, "{\n");
		if(joint.parent_idx != ROBOT_NOPARENT_NODE)
		{
			fprintf(file, "\tparent %s\n", joints[joint.parent_idx].name.c_str());
		}
		
		
		if(!joint.T0.is_rotate_zero())
		{
			float axis[3];
			float angle;
			joint.T0.extract_axis_angle(axis, angle);
			fprintf(file, "\tConstRot %f %f %f %f\n", axis[0], axis[1], axis[2], angle);
		}
		
		if(!joint.T0.is_transl_zero())
		{
			fprintf(file, "\tConstTransl %f %f %f\n", joint.T0.m_T[0], joint.T0.m_T[1], joint.T0.m_T[2]);
		}
		
		switch(joint.joint_type)
		{
		case JT_PRISMATIC:
			fprintf(file, "\tTransl1 %f %f %f %f %f\n", joint.axis[0], joint.axis[1], joint.axis[2], joint.j_min, joint.j_max);
			break;
		case JT_REVOLUTE:
			fprintf(file, "\tRot1 %f %f %f %f %f\n", joint.axis[0], joint.axis[1], joint.axis[2], joint.j_min, joint.j_max);
			break;
		case JT_CONTINUOUS:
			fprintf(file, "\tRot1 %f %f %f %f %f\n", joint.axis[0], joint.axis[1], joint.axis[2], joint.j_min, joint.j_max);
			break;
		case JT_FIXED:
			fprintf(file, "\tRot1 %f %f %f %f %f\n", 1.0f, 0.0f, 0.0f, 0.0f, 0.0f);
			break;
		case JT_UNKNOWN:
			break;
		}
		
		if(joint.model)
		{
			unsigned int begin = joint.model->modelFileName.rfind("/");
			std::string filename = joint.model->modelFileName.substr(begin + 1);
			filename = filename + ".obj";
			fprintf(file, "\tmodel0 \"%s\"\n", filename.c_str());
			filename = filename;
			joint.model->saveOBJ(filename.c_str());
		}
		
		fprintf(file, "\tparam %d\n", i);
		
		
		fprintf(file, "}\n");
		fprintf(file, "\n");
	}
	
	fclose(file);
}